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Abstract — Optimal control problems are formulated and 
efficient computational procedures are proposed for combined 
orbital and rotational maneuvers of a rigid body in three 
dimensions. The rigid body is assumed to act under the 
influence of forces and moments that arise from a potential and 
from control forces and moments. The key features of this paper 
are its use of computational procedures that are guaranteed to 
preserve the geometry of the optimal solutions. The theoretical 
basis for the computational procedures is summarized, and 
examples of optimal spacecraft maneuvers are presented. 

I. Introduction 

Discrete optimal control problems for translational and 
rotational dynamics of a rigid body under a potential are 
studied. Optimal control of a rigid body arises in numerous 
engineering and scientific fields. These problems provide 
both a theoretical challenge and a numerical challenge in the 
sense that the configuration space has a Lie group structure 
denoted by SE(3) that defines a fundamental constraint. 

Optimal control problems on a Lie group have been 
studied in [1], [2]. These studies are based on the driftless 
kinematics of a Lie group. The dynamics are ignored, and 
it is assumed that elements in the corresponding Lie algebra 
are controlled directly. 

General-purpose numerical integration methods, includ- 
ing the popular Runge-Kutta schemes, typically preserve 
neither the group structure of the configuration space nor 
geometric invariants of the dynamics. Geometric structure- 
preserving integrators, referred to as Lie group variational 
integrators [3], preserve the group structure without the use 
of local charts, reprojection, or constraints, and they have the 
desirable property that they are symplectic and momentum 
preserving, and they exhibit good energy behavior for an 
exponentially long time period. 

This paper presents geometrically exact and numerically 
efficient computational approaches to solve optimal control 
problems of a rigid body on a Lie group, SE(3). The 
dynamics and the kinematics are discretized by a Lie group 
variational integrator, and discrete optimality conditions are 
constructed. Efficient numerical algorithms to solve the 
necessary condition are developed. This method provide 
a substantial advantage over current methods for optimal 
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control on a Lie group in the sense that the dynamics of a 
rigid body as well as the kinematics equation are explicitly 
utilized, and the proposed computational approaches respect 
the group structure. 

This paper is organized as follows. In Section |ll| a Lie 
group variational integrator is developed. Optimal control 
problems using impulsive controls are studied in Section 
Unl and optimal control problems with smooth controls are 
studied in Section |IV| Numerical results for a rigid dumbbell 
spacecraft are given in Section |V| 

II. Lie group variational integrator on SE(3) 

The configuration space for the translational and rotational 
motion of a rigid body is the special Euclidean group, 
SE(3) = R^(s)S0(3). We identify the cotangent bundle 
T*SE(3) with SE(3) x se(3)* by left translation, and we 
identify se(3)* with by an isomorphism between and 
se(3), and the standard inner product on R^. We denote the 
attitude, position, angular momentum, and linear momentum 
of the rigid body by [R, x, n, 7) G T*SE(3). 

The continuous equations of motion are given by 
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where G R^ is the angular velocity, and , G R^ are 
the control force in the inertial frame and the control moment 
in the body fixed frame, respectively. The constant mass of 
the rigid body is m G R, and J G R^ ^ ^ denotes the moment 
of inertia, i.e. 11 = JQ. The map S{-) : R^ ^ 50 (3) is an 
isomorphism between 50 (3) and R^ defined by the condition 
S{x)y = X X y for all x,y e R^. 

We assume that the potential is dependent on the position 
and the attitude; U{-) : SE(3) k^R. The corresponding force 
and the moment due to the potential are given by 



dx ' 



M 



where , Ur 



= ri X Ur:^ + r2 X + ^3 X ^r3, 

G R^ are the ith row vector of R and 
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respectively. 

Since the dynamics of a rigid body has the structure of 
a Lagrangian or Hamiltonian system, they are characterized 



by symplectic, momentum and energy preserving properties. 
These geometric features determine the quaUtative behavior 
of the rigid body dynamics, and they can serve as a basis 
for theoretical study of rigid body dynamics. 

In contrast, the most common numerical integration meth- 
ods, including the widely used Runge-Kutta schemes, nei- 
ther preserve the Lie group structure nor these geometric 
properties. In addition, standard Runge-Kutta methods fail 
to capture the energy dissipation of a controlled system 
accurately [4]. Additionally, if we integrate 01) by a typical 
Runge-Kutta scheme, the quantity R^R inevitably drifts 
from the identity matrix as the simulation time increases. 
It is often proposed to parameterize Q by Euler angles 
or unit quaternions. However, Euler angles are not global 
expressions of the attitude since they have associated singu- 
larities. Unit quaternions do not exhibit singularities, but are 
constrained to lie on the unit three-sphere S^, and general 
numerical integration methods do not preserve the unit length 
constraint. Therefore, quaternions have the same numerical 
drift problem. Renormalizing the quaternion vector at each 
step tends to break other conservation properties. Further- 
more, unit quaternions, which are diffeomorphic to SU(2), 
double cover SO (3). So there are inevitable ambiguities in 
expressing the attitude. 

In [3], Lie group variational integrators are introduced by 
explicitly adapting Lie group methods [5] to the discrete 
variational principle [4]. They have the desirable property 
that they are symplectic and momentum preserving, and 
they exhibit good energy behavior for an exponentially long 
time period. They also preserve the Euclidian Lie group 
structure without the use of local charts, reprojection, or 
constraints. These geometrically exact numerical integration 
methods yield highly efficient and accurate computational 
algorithms for rigid body dynamics. They avoid singularities 
and ambiguities. 

Using the results presented in [6], a Lie group variational 
integrator on SE(3) for equations ([l}-© is given by 

h / \ 

Xk+l =Xk^ 7/c + 7^ f/fe +^{) , (7) 

m 2m V / 

7/c+i = 7fe + - (/fe + uQ + - (^/fe+i + ul^^^ , (8) 

hS{Uk + ^ (Mfc + u^)) = FkJd - JdFl, (9) 
= i^feFfe, (10) 

(11) 

where the subscript k denotes the /cth discrete variables 
for a fixed integration step size G M. J(i G M^^^ is 
a nonstandard moment of inertia matrix defined by Jd = 
|tr[J] Isxs — J- Fk G SO (3) is the relative attitude between 
adjacent integration steps. 

For given {R^^ Xk^Hk^lk) and control inputs, ^ is 
solved to find F^. Then {Rk-\-ijXk-\-i) are obtained by 
JToll.ilTt. Using (111),©, (/fc+i, M/c+i) are computed, and 
they are used to find (II/c+i, 7/^+1 ) by fTTl.ilSl). This yields 



a map {Rk, Xk.Uk.jk) ^ ^/c+i, Il/e+i, 7/e+i), and 

this process is repeated. The only implicit part is The 
actual computation of F^ is done in the Lie algebra 50 (3) 
of dimension 3, and the rotation matrices are updated by 
multiplication. This approach is completely different from in- 
tegration of the kinematics equation Q; there is no excessive 
computational burden. It can be shown that this integrator 
has second order accuracy. The properties of these discrete 
equations of motion are discussed in more detail in [3], [6]. 

III. Optimal Impulsive Control of a Rigid Body 

We formulate an optimal impulsive control problem for a 
rigid body on SE(3), and we develop sensitivity derivatives. 
They are used in our computational method for solve optimal 
impulsive control problems. 

A. Problem formulation 

An optimal impulsive control problem is formulated as a 
maneuver of a rigid body from a given initial configuration 
(i?o, ^0, Ho, 70) to a desired configuration described by 

{{Rn.xn, n^, 7^) e T*SE(3) \C{Rn, xn, I^nHn) = 0} , 

where C(-) : T*SE(3) ^ W during the given maneuver 
time TV. Two impulsive control inputs are applied at the 
initial time and the terminal time. We assume that the control 
inputs are purely impulsive, which means that each impulse 
changes the momentum of the rigid body instantaneously, but 
it does not have any effect on the position and the attitude of 
the rigid body at that instant. The motion of the rigid body 
between the initial time and the terminal time is uncontrolled, 
i.e. ul = = 0. The performance index is the sum of the 
magnitudes of the initial impulse and the terminal impulse. It 
is equivalent to minimizing the sums of the initial momentum 
change and the terminal momentum change. 

We transform this optimal impulsive control problem into 
a parameter optimization problem. Let (nQ",7Q") be the 
initial momentum after the initial impulsive control. Then, 
the terminal states are determined by the discrete equations 
of motion, and the momentum after the terminal impulsive 
control, (n^,7^), can be computed by the terminal con- 
straint. Therefore, the performance index and the constraint 
are completely determined by (IIq",7q"). Thus, the optimal 
impulsive control on SE(3) is formulated as 

given : (i?o, ^0, Hq, 70), iV 
min J = llnj - IIoll + ||7o" - 7o|| 

+ ||n^ - IlArll + ||7^ -7iv|| 1 
such that C{Rn,xn,^n^^n) = 0' 
subject to discrete equations of motion Q-CD]). 

If the desired values for all of the terminal states are specified 
by the constraints, then there is no freedom for optimization. 
This problem degenerates to a two point boundary value 
problem on SE(3), which can be considered as an extension 
of the Lambert problem for the restricted two body problem. 
A similar optimal control problem for attitude dynamics of 
a rigid body on SO (3) is studied in [7]. 



B. Sensitivity derivatives 

Variational model: The variation of Qk = (Rk^Xk) G 
SE(3) can be expressed in terms of a Lie algebra element 
r]k ^ 5e(3) and the exponential map as = exp er]k- The 
corresponding infinitesimal variation is given by 

d 
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Using homogeneous coordinates [8], the above equation is 
written in a matrix equation as 
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where Cfe,Xfe ^ so that (5'(C/c), X/c) ^ 5e(3). This gives 
an expression for the infinitesimal variation of a Lie group 
element in terms of its Lie algebra. Then, small perturbations 
from a given trajectory on T*SE(3) can be written as 



xl= Xk^ eSxk, 
Ik = Ik^eSjk, 

Rl = Rk^eRkS{Ck)^0{e^), 



(13) 
(14) 
(15) 
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where Sxkj Sjk^ ^^kXk are considered in R^. 

We derive expressions for the constrained variation of 
using ([Toll and (O. Since Fk = R^Rk+i by ([To|l, the 
infinitesimal variation SFk is given by 

SFk = SR^Rk-\-i + R^SRk-\-i = —S{(k)Fk + FkS{Ck-\-i)' 

We can also express SF^ = FkS{S,k) for S,k G M^, using (IT2l . 
Using the property ^(i^^x) = R^S{x)R for all G S0(3) 
and X G M^, we obtain the constrained variation of Fk 



(17) 



Linearized equations of motion: Substituting the variation 
model (IT3l-(IT6l and the constrained variation dTTt into the 
equations of motion (TTl-dTTl. and ignoring higher order 
terms, the linearized equation of motion can be written as 



Zk-\-i = AkZk^ 



(18) 



where Zk = [Sxk; Sjk] Ck] ^^k] ^ 1^^^ and Aj, G R^2xi2 
can be suitably defined. The solution of dTsl is obtained as 



ZN ^Zo, 



(19) 



where $ G R^^^^^ represents the sensitivity derivatives of 
the terminal state with respect to the initial state on SE(3). 

C. Computational approach 

We solve the optimal impulsive control problem by the 
Sequential Quadratic Programming (SQP) method using ana- 
lytical expressions for the gradients of the performance index 
and the constraints. The exact computation of the gradients 
are crucial for efficient numerical optimization. For the given 



problem, Sxo = Co = since the initial position and the 
initial attitude are fixed. Thus, (IT9l is written as 
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where G R^^^, i,j G (1,2,3,4) are submatrices of 
The above equation represents the sensitivities of the ter- 
minal state with respect to the initial momentum (nQ",7Q"). 
Therefore, we can obtain expressions for gradients of the 
performance index and the constraints, and any Newton type 
numerical approach can be applied. 

IV. Optimal Control of a Rigid Body 

We formulate an optimal control problem for a rigid 
body on SE(3) assuming that control forces and moments 
are applied during the maneuver. Necessary conditions for 
optimality are developed and computational approaches are 
presented to solve the corresponding two point boundary 
value problem. 

A. Problem formulation 

An optimal impulsive control problem is formulated 
as a maneuver of a rigid body from a given initial 
configuration (i?o, ^o, Hq, 70) to a desired configuration 
{R%j x%^ n^, 7^) during the given maneuver time N. Con- 
trol inputs are parameterized by their value at each time step. 
The performance index is the square of the weighted I2 norm 
of the control inputs. 

given: (xq, 70, i^o, Hq), {x% , ^% , R% ,11%) , N, 

N-l , , 



/c=0 

such that {xn^n^Rn^^n) = {x%,jf^,R%,U%), 
subject to discrete equations of motion (l7b-(ITTl. 

where Wf , Wm ^ R^ ^ ^ are symmetric positive definite 
matrices. Here we use a modified version of the discrete 
equations of motion with first order accuracy, because it 
yields a compact form for the necessary conditions, which 
are developed the following subsection. A similar optimal 
control problem for attitude dynamics on SO (3) is studied 
in [9]. 

B. Necessary conditions for optimality 

Define an augmented performance index as 



k=0 



-Xfe+i + X/e H 7fe 

m 



+ Afc^ {-7fc+i +lk + hfk+i + 

+ Xl:^S-' (logm(Ffc - RlRk+i)) 

+ {-Hfe+i + F^Uk + h {Mk+i + } , 



where G are Lagrange multipliers. The constraint ^ 
is considered implicitly using a constrained variation. Using 
the variational model (IT3l-(IT6l). the constrained variation 
(ITtI . and the fact that the variations vanish at /c = 0, A^, 
we obtain the infinitesimal variation of Ja as 

N-l 
k=l 

where A^ = [A^ ; A|; A|; A^] e R^^ and e Ri2xi2 
presented in dTsl . 

Since = for all variations, we obtain necessary 
conditions for optimality as follows. 



h 

Xk+i = Xk-\ 7fe, 
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(21) 


7fe+i = 7fc + hfk+i + hu{j^^, 


(22) 


hS{nu) = FkJd - JdF^, 


(23) 


Rk+l = RkFk, 


(24) 


Uk+i = F^Uk + hMk+i + huT+i, 


(25) 




(26) 


<+i = -w-'\t, 


(27) 


Xk = ^fe+iAfe+i. 


(28) 



In the above equations, the only implicit part is (l23l . For a 
given initial condition (i?o, ^o, Ho, 70) and Aq, we can find 
Fo by solving Then, Ri.xi is obtained by (I24l.(l271. 
and the control input u{^u^ is obtained by (l26l.(l27l. 
71, El can be obtained by (l22li.(l25l. Now we compute 
xi, Hi, 71). We solve ^ to find Fi. Finally, Ai can be 
obtained by (l28l . This yields a map {(i^o, ^o, Hq, 70), Aq} ^ 
xi, Hi, 7i), Ai}, and this process can be repeated. 

C. Computational Approach 

The necessary conditions for optimality are expressed in 
terms of a two point boundary problem on T*SE(3) and 
its dual. This problem is to find the optimal discrete flow, 
multiplier, and control inputs to satisfy the equations of 
motion (I2ni-(l25l. optimality conditions (l26l.(l27l. multiplier 
equations (l28l . and boundary conditions simultaneously. 

We use a neighboring extremal method [10]. A nominal 
solution satisfying all of the necessary conditions except 
the boundary conditions is chosen. The unspecified initial 
multiplier is updated by successive linearization so as to 
satisfy the specified terminal boundary conditions in the 
limit. This is also referred to as a shooting method. The main 
advantage of the neighboring extremal method is that the 
number of iteration variables is small. In other approaches, 
the initial guess of control input history or multiplier vari- 
ables are iterated, so the number of optimization parameters 
are proportional to the number of discrete time steps. 

The difficulty is that the extremal solutions are sensitive 
to small changes in the unspecified initial multiplier values. 
The nonlinearities also make it hard to construct an accurate 



estimate of sensitivity, and it may result in numerical ill- 
conditioning. Therefore, it is important to compute the sensi- 
tivities accurately to apply the neighboring extremal method. 

Here the optimality conditions (l26l and (l27l are substi- 
tuted into the equations of motion and the multiplier equa- 
tions. The sensitivities of the specified terminal boundary 
conditions with respect to the unspecified initial multiplier 
conditions is obtained by a linear analysis. 

Similar to dTsl . the linearized equations of motion can be 
written as 

Zfc+i = AkZk^A^'^dXk, (29) 

where Af = -hdid^g[0,Wf\0,W-^ e Ri2xi2^ 
linearize the multiplier equations (l28l to obtain 

5Xk = Al\iZk^i + Al^^5Xk^i, (30) 

where Al\i G R^^^^^ can be defined properly. The solution 
of the linear equations (l29l and (l3Ql can be obtained as 
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where G R^^^^^ 

For the given two point boundary value problem zq = 
since the initial condition is fixed, and A at is free. Thus, 

ZN = ^12(^Ao. (31) 

The matrix ^12 represents the sensitivity of the specified 
terminal boundary conditions with respect to the unspecified 
initial multipliers. Using this sensitivity, an initial guess of 
the unspecified initial conditions is iterated to satisfy the 
specified terminal conditions in the limit. 

Any type of Newton iteration can be applied. We use a line 
search with backtracking algorithm, referred to as Newton- 
Armijo iteration in [11]. The procedure is summarized as 
follows. 



1: 


Guess an initial multiplier Aq. 


2: 


Find x/c, 7/c, Il/e, i?/c, Xk using 


3: 


Compute the terminal B.C. error; Error = \\zn\\- 


4: 


Set Error^ = Error, i = 1. 


5: 


while Error > e^. 


6: 


Find a line search direction; D = "^12 • 


7: 
8: 


Set c = 1. 

while Erro/ > (1 — 2ac) Error 


9: 


Choose a trial multiplier Aq = Aq + cDzn- 


10: 


Find x/e, 7/c, n^, i?^, A^ using 


11: 


Compute the error; Error^ = \\z\^\\. 


12: 


Set c = c/10, z = i + l. 


13: 


end while 


14: 


Set Ao = Aq, Error = Error^ (accept the trial) 


15: 


end while 



Here i is the number of iterations, and 65,0^ G R are a 
stopping criterion and a scaling factor, respectively. The outer 
loop finds a search direction by computing the sensitivity 
derivatives, and the inner loop performs a line search to find 
the largest step size c G R along the search direction. The 
error in satisfaction of the terminal boundary condition is 
determined at each inner iteration. 



V. Numerical Examples 

A. Restricted Full Two Body Problem 

We study a maneuver of a rigid spacecraft under a central 
gravity field. We assume that the mass of the spacecraft is 
negligible compared to the mass of a central body, and we 
consider a fixed frame attached to the central body as an 
inertial frame. The resulting model is a Restricted Full Two 
Body Problem (RF2BP). 

The spacecraft is modeled as a dumbbell, which consists 
of two equal spheres and a massless rod. The gravitational 
potential is given by 

GMm ^ 1 
U(x,R) = — > — (32) 

where G G M is the gravitational constant, M, m G M are 
the mass of the central body, and the mass of the dumbbell, 
respectively. The vector G is the position of the qth 
sphere from the mass center of the dumbbell expressed in the 
body fixed frame (q G {1,2}). The mass, length, and time 
dimensions are normalized by the mass of the dumbbell, the 
radius of a reference circular orbit, and its orbital period. 

B. Optimal Impulsive Control 

We study an impulsive orbital transfer problem with an 
attitude change. Initially, the spacecraft is on a reference 
circular orbit. We consider two cases. In the first case, the 
spacecraft moves to a desired circular orbit and the desired 
values for all of the terminal state are specified. There is no 
freedom for optimization, and the resulting problem is a two 
point boundary value problem on SE(3). This maneuver can 
be considered as a generalization of Hohmann transfer [12]. 
The desired maneuver involves doubling the orbital radius in 
addition to a large angle attitude change. 

In the second case, the terminal constraints are relaxed 
such that the spacecraft is allowed to transfer to any point 
on the desired orbit. The desired terminal orbit is described 
by its orbital radius rd G M, and a directional vector G 
normal to the orbital plane. Two constraints are imposed 
to locate the dumbbell in the desired orbital plane with the 
desired orbital radius, and one constraint is applied to align 
the dumbbell to the normal direction. 

The gradients of the performance index and the constraints 
are obtained by using (|2Q|) . We use Matlab f mi neon func- 
tion as an implementation of the SQP algorithm. Figures [l] 
and 121 show the spacecraft maneuver, and linear velocity and 
angular velocity responses, where red circles denote the ve- 
locities before the initial impulse and the velocities after the 
terminal impulse. Thus, differences between solid lines and 
red circles are proportional to the impulsive controls. (Simple 
animations which show these maneuvers of the spacecraft 
can be found at http://www.umich.edu/~tylee) 
The error in satisfaction of the terminal boundary value of 
the first case is4.77xl0~^^. The performance index and the 
maximum violations of the constraints for the second case 
are 1.2305 and 3.88 x 10-^^ respectively. 



C Optimal Control 

We study an optimal orbital transfer problem to increase 
the orbital inclination by 60deg, and an orbital capture 
problem to the reference circular orbit. 

Figures [3] and |4| show the optimized spacecraft maneuver, 
control inputs history. For each case, the performance indices 
are 13.03 and 20.90, and the maximum violations of the 
constraint are 3.35 x 10~^^ and 3.26 x 10~^^, respectively. 

Figures [3|(b) and|4l(b) show the violation of the terminal 
boundary condition according to the number of iterations in 
a logarithmic scale. Red circles denote outer iterations in 
Newton- Armijo iteration to compute the sensitivity deriva- 
tives. For all cases, the initial guesses of the unspecified 
initial multiplier are arbitrarily chosen. The error in satisfac- 
tion of the terminal boundary condition converges quickly 
to machine precision after the solution is close to the local 
minimum at around 20th iteration. These convergence results 
are consistent with the quadratic convergence rates expected 
of Newton methods with accurately computed gradients. 

The neighboring extremal method, also referred to as the 
shooting method, is numerically efficient in the sense that the 
number of optimization parameters is minimized. But, this 
approach may be prone to numerical ill-conditioning [13]. A 
small change in the initial multiplier can cause highly nonlin- 
ear behavior of the terminal attitude and angular momentum. 
It is difficult to compute the gradient for Newton iterations 
accurately, and the numerical error may not converge. 

However, the numerical examples presented in this paper 
show excellent numerical convergence properties. This is 
because the proposed computational algorithms on SE(3) are 
geometrically exact and numerically accurate. 

The dynamics of a rigid body arises from Hamiltonian 
mechanics, which have neutral stability, and its adjoint 
system is also neutrally stable. The proposed Lie group 
variational integrator and the discrete multiplier equations, 
obtained from variations expressed in the Lie algebra, pre- 
serve the neutral stability property numerically. Therefore the 
sensitivity derivatives are computed accurately. 

VI. Conclusions 

Optimal control problems for combined orbital and rota- 
tional maneuvers of a rigid body are formulated and efficient 
computational procedures are proposed. The dynamics are 
discretized by a Lie group variational integrator, and sensi- 
tivity derivatives are developed by a linear analysis. Discrete 
necessary conditions for optimality are constructed, and the 
corresponding two point boundary value problem is solved 
efficiently. 

This approach is geometrically exact in the sense that the 
Lie group variational integrator preserves the group structure 
as well as the geometric invariant properties, and the sensi- 
tivity derivatives are expressed in terms of its Lie algebra. 
Since the configuration of a rigid body is defined globally 
using an element of SE(3), this approach completely avoids 
singularity or ambiguity arising from other representations 
such as Euler angles and quaternions. Numerical examples 
show the efficiency of the proposed computational approach. 
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Fig. 1. TPBVP: Orbital radius change 
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Fig. 2. Optimal impulsive control: Orbital radius change 
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Fig. 3. Optimal control: Orbital inclination change 
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Fig. 4. Optimal control: Orbital capture 



